#include "app_includes.h"

static void print_matrix(int n, double mat[]);

void matrix_test (void){
	int i,error = 0;
	TSK_Ctrl_b *self = &(SCB.TCB[MATRIX_TEST]);
	int n = 15;
	int print = 0;
	double mat2[225];
	double mat1[225] =
	{1,1,5,5,5,5,4,2,4,3,2,3,1,3,1,
	 4,1,1,4,3,2,5,2,4,2,3,2,4,2,2,
	 5,1,1,4,5,1,4,2,1,2,3,4,3,2,3,
	 1,5,2,2,4,4,4,4,1,5,1,2,1,1,3,
	 4,5,4,1,2,1,2,4,3,4,3,1,2,3,3,
	 1,1,5,2,4,2,5,5,1,5,3,3,2,5,2,
	 2,5,3,4,2,2,2,5,2,3,5,3,5,1,5,
	 1,4,1,5,5,1,1,3,2,4,5,2,4,1,4,
	 2,5,2,4,3,1,4,4,5,1,5,1,5,1,5,
	 2,3,2,3,1,3,5,1,2,1,4,5,4,5,1,
	 1,3,1,5,4,5,1,3,2,2,3,2,4,1,4,
	 2,2,2,5,3,5,3,3,3,2,4,2,3,5,2,
	 5,1,4,4,2,1,1,5,5,3,1,1,5,3,3,
	 2,2,1,4,4,4,3,5,1,5,1,4,1,5,1,
	 3,1,4,5,1,5,5,3,3,2,3,3,3,1,3};

	vTskTime(self,RESET_TIME);
	if(print){
		printf("mat1: \n");
		print_matrix(n,mat1);
	}

    for(;;){

	    SEM_pend(&matrix_sem, SYS_FOREVER);
#ifdef _DEBUG
	    STS_set(&sts_MatrixInv,SCB.TIME_STAMP);
#endif
	    vTskTime(self,SET_PREV_TIME);
		//for(i=0;i<1;i++){
			error = MatInverse(n,mat1,mat2);
		//}
		//TSK_sleep(ms_conv_to_tick(1));
		vTskTime(self,DELTA_TIME);
#ifdef _DEBUG
		STS_delta(&sts_MatrixInv,SCB.TIME_STAMP);
#endif
		if(error){
			printf("error on inverse\n");
		}else{
			if(print){
				print_matrix(n,mat2);
			}
		}
		TSK_yield();
	}
}

static void print_matrix(int n, double mat[]){
	int i,j = 0;
	for(i=0;i<n;i++){
		for(j=0;j<n;j++){
			printf("%f ",mat[i*n+j]);
		}
		printf("\n");
	}
	printf("\n");
}
